#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#一条规划，回到middle位置再送食物，会掉电，所以延时后回到middle
import sys
import rospy
import moveit_commander
import geometry_msgs.msg
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint


def scale_trajectory_speed(traj, scale):
       
       # Create a new trajectory object
       new_traj = RobotTrajectory()
       
       # Initialize the new trajectory to be the same as the input trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
 
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
       
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
        
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
       
       # Cycle through all points and joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj

def move_to_multiple_targets():
    # 初始化 moveit_commander 和 rospy
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface', anonymous=True)

    # 初始化机器人手臂的 move group
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 获取初始位置
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 定义多个目标位姿
    waypoints = []


    # 第1个目标位姿 0/-50/90/15/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = -0.023623156207669485
    pose1.position.y = -5.61926355873778e-06
    pose1.position.z = 0.1061805918499025
    pose1.orientation.x = -2.4631350004671237e-05
    pose1.orientation.y = 0.4616371139275134
    pose1.orientation.z = -7.937233310081275e-06
    pose1.orientation.w = 0.8870688667600578
    waypoints.append(pose1)

    # 2 0/-70/120/-12/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.02270275459045043
    pose2.position.y = -3.0206178897082266e-05
    pose2.position.z = 0.08515341454770431
    pose2.orientation.x = -3.0504780108955957e-05
    pose2.orientation.y = 0.32557952141613367
    pose2.orientation.z = -8.924993019861189e-05
    pose2.orientation.w = 0.9455146568606697
    waypoints.append(pose2)

    # 3 0/-65/121/-18/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = 0.03716382693672583
    pose3.position.y = -1.5665732298609626e-05
    pose3.position.z = 0.08467326062547971
    pose3.orientation.x = -2.6268154351170503e-05
    pose3.orientation.y = 0.3256253211597455
    pose3.orientation.z = -4.6484770819418694e-05
    pose3.orientation.w = 0.945498888084361
    waypoints.append(pose3)

    # 第4个目标位姿 0/-68/128/-27/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.05279465803518045
    pose4.position.y = 2.85229202356785e-05
    pose4.position.z = 0.08403403724734092
    pose4.orientation.x = -1.0133215588657257e-05
    pose4.orientation.y = 0.2839328542037646
    pose4.orientation.z = 7.950130493131293e-05
    pose4.orientation.w = 0.9588441624584073
    waypoints.append(pose4)

    # 第5个目标位姿 0/-70/135/-33/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.059453244311609865
    pose5.position.y = -8.330909781430266e-06
    pose5.position.z = 0.07075068945087021
    pose5.orientation.x = -2.782322372343405e-06
    pose5.orientation.y = 0.2755682005866627
    pose5.orientation.z = -2.508720262445286e-05
    pose5.orientation.w = 0.9612815228580647
    waypoints.append(pose5)

    # 第6个目标位姿 0/-70/141/-50/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.08872055239306093
    pose6.position.y = 0.00012314349764190854
    pose6.position.z = 0.09403262420395148
    pose6.orientation.x = -1.2451783763174253e-05
    pose6.orientation.y = 0.1823097069171027
    pose6.orientation.z = 0.0003162268136817005
    pose6.orientation.w = 0.9832411050242742
    waypoints.append(pose6)

    # # 第7个目标位姿 0/-72/150/-78/0 举平
    #0/-72/142/-78/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.11482399330409013
    pose7.position.y = 2.1035317438294303e-05
    pose7.position.z = 0.1529639014580961
    pose7.orientation.x = 1.8613930079306421e-06
    pose7.orientation.y = 0.0001556714793681703
    pose7.orientation.z = 8.278564739240959e-05
    pose7.orientation.w = 0.9999999844547312
    waypoints.append(pose7)

    # # 第9个目标位姿 16/-72/150/-78/0 举平移
    #16/-75/142/-80/0
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.10568869680086797
    pose9.position.y = 0.06496941623114455
    pose9.position.z = 0.1529569397066705
    pose9.orientation.x = -2.5174032480959837e-05
    pose9.orientation.y = 0.0001925602901853463
    pose9.orientation.z = 0.13929909717987762
    pose9.orientation.w = 0.9902503339114176
    waypoints.append(pose9)

    # 第10个目标位姿 middle
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.11075168589927481
    pose10.position.y = -1.091277667118024e-05
    pose10.position.z = 0.2875988062936282
    pose10.orientation.x = 1.8382147442206865e-06
    pose10.orientation.y = 8.132110012412454e-05
    pose10.orientation.z = -2.4453694472133078e-05
    pose10.orientation.w = 0.9999999963927584
    waypoints.append(pose10)

    # 第11个目标位姿 0/40/0/-40/0
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.3035704582748756
    pose11.position.y = -4.6019088696090045e-05
    pose11.position.z = 0.30536264212765296
    pose11.orientation.x = 1.8345570569793093e-06
    pose11.orientation.y = -7.180863568924419e-05
    pose11.orientation.z = -6.589662785214818e-05
    pose11.orientation.w = 0.9999999952488945
    waypoints.append(pose11)



    # 规划并执行笛卡尔轨迹
    (plan, fraction) = arm_group.compute_cartesian_path(
        waypoints,   # 目标位姿序列
        0.01,        # eef_step，终端执行器的步长
        False)         # jump_threshold，跳跃阈值
    print("Fraction of path achieved: ", fraction)

    # 执行轨迹
    new_plan=scale_trajectory_speed(plan,1.2)
    arm_group.execute(new_plan, wait=True)

    # 停止运动
    arm_group.stop()

    # 清空之前的目标
    arm_group.clear_pose_targets()
    
    # rospy.sleep(3)
    # arm_group.set_named_target("middle")
    # plan = arm_group.go(wait=True)

    # 关闭并退出 moveit_commander
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

if __name__ == '__main__':
    try:
        move_to_multiple_targets()
    except rospy.ROSInterruptException:
        pass
